package es.uji.viselab.visualservoing;

import javax.swing.JPanel;

import es.uji.viselab.experiment.Experiment;
import es.uji.viselab.experiment.ExperimentParameters;
import es.uji.viselab.image.camera.CameraCatalog;
import es.uji.viselab.image.process.ImageProcessing;
import es.uji.viselab.image.source.ImageSource;
import es.uji.viselab.math.Vector2;
import javax.vecmath.Vector2d;

public class SimpleIBVS extends VisualServoingTask {

	public SimpleIBVS(ExperimentParameters ep) {
		super(ep);
	}

	@Override
	public double[] compute(ImageSource image) {
		
		//Experiment experiment = image.getExperiment();
		CameraCatalog camera = ep.getCamParameters();
		double df = camera.getFocalDistanceCorrected();
		
		 Vector2[] targetCoordinateList;
		 Vector2[] imageCoordinateList;

		
		targetCoordinateList = image.getTargetCoordinateList();
		imageCoordinateList = image.getImageCoordinateList();

		// Where is the center of both coordinate-list?
		double[] targetCenter = center(targetCoordinateList[0].x,targetCoordinateList[0].y,targetCoordinateList[2].x,targetCoordinateList[2].y );
		double[] imageCenter=center(imageCoordinateList[0].x,imageCoordinateList[0].y,imageCoordinateList[2].x,imageCoordinateList[2].y );
		//System.out.println("Target Center: " + targetCenter[0] + ", "+ targetCenter[1]);
		//System.out.println("Image Center: " + imageCenter[0] + ", "+ imageCenter[1]);
		
		double distanceX = imageCenter[0]-targetCenter[0];
		double distanceY = imageCenter[1]-targetCenter[1];
		//System.out.println("Distance: " + distanceX + ", "+ distanceY);
		
		double depthStimation = 0.1; // = 100 cm
		
		double[] velocities = {0,0,0,0,0,0};
		
		
		velocities[0]=depthStimation*distanceX/df;
		velocities[1]=depthStimation*distanceY/df;
			
		// Is the target far or nearer ?
		// This is to calculate the translation in the Z coordinate
		double perimeterTarget=0;
		
		for (int i=0;i<3;i++){
			perimeterTarget=perimeterTarget+distance(targetCoordinateList[i].x,targetCoordinateList[i].y,targetCoordinateList[i+1].x,targetCoordinateList[i+1].y);
		}
		perimeterTarget=perimeterTarget+distance(targetCoordinateList[3].x,targetCoordinateList[3].y,targetCoordinateList[0].x,targetCoordinateList[0].y);
		
		double perimeterImage=0;
		for (int i=0;i<3;i++){
			perimeterImage=perimeterImage+distance(imageCoordinateList[i].x,imageCoordinateList[i].y,imageCoordinateList[i+1].x,imageCoordinateList[i+1].y);
		}
		perimeterImage=perimeterImage+distance(imageCoordinateList[3].x,imageCoordinateList[3].y,imageCoordinateList[0].x,imageCoordinateList[0].y);
		
		if (perimeterImage>perimeterTarget)
			velocities[2]=-depthStimation * (perimeterImage/perimeterTarget);
		if (perimeterImage<=perimeterTarget)
			velocities[2]=depthStimation * (perimeterTarget/perimeterImage);
		velocities[2]=velocities[2]/(Math.abs(distanceX)+Math.abs(distanceY)+1);
		//System.out.println("Perimeter image: "+ perimeterImage+ ", " + "PerimeterTarget: "+ perimeterTarget);
		double correction = 0;
		if (perimeterImage>perimeterTarget)
			correction = (perimeterImage/perimeterTarget);
		if (perimeterImage<=perimeterTarget)
			correction = (perimeterTarget/perimeterImage);
		velocities[0]=velocities[0]*correction;
		velocities[1]=velocities[1]*correction;
		// now time to calculate the rotation among the Z coordinate
		Vector2d v1 = new Vector2d();
		Vector2d v2 = new Vector2d();
		v1.x=targetCoordinateList[0].x-targetCoordinateList[1].x;
		v1.y=targetCoordinateList[0].y-targetCoordinateList[1].y;
		v2.x=imageCoordinateList[0].x-imageCoordinateList[1].x;
		v2.y=imageCoordinateList[0].y-imageCoordinateList[1].y;
		double theta = getAngle(v1,v2);
		theta = theta*360/(2*Math.PI);
		if (theta<0)
			theta = 360+theta;
		theta = theta % 90;
		if (theta >45)
			theta = 90- theta;
		if (theta>10)
			velocities[5]=theta*0.1*-1;
		else if (theta>1)
		    velocities[5]=theta*0.5*-1;
		else 
			velocities[5]=0;
		//velocities[5]=0;
		
		System.out.println("IBVS Velocities: " + velocities[0] + ", "+ velocities[1] + ", "+ velocities[2] + ", "+ velocities[5] );
		//System.out.println("-------------------------------------" );
	
		return velocities;
        
	}

	private double getAngle(Vector2d v1, Vector2d v2) {
		double angle1 = Math.atan2(v1.y, v1.x);
		double angle2 = Math.atan2(v2.y, v2.x);
		return angle1 - angle2;
	}

	private double distance(double x1, double y1, double x2, double y2) {
		double vectorX = x2 - x1;
		double vectorY = y2 - y1;
		return Math.sqrt(Math.pow(vectorX, 2) + Math.pow(vectorY, 2));
	}

	private double[] center(double x1, double y1, double x2, double y2) {
		double vectorX = x2 - x1;
		double vectorY = y2 - y1;
		double[] xy = new double[2];
		xy[0] = x1 + vectorX / 2;
		xy[1] = y1 + vectorY / 2;
		return xy;
	}

	@Override
	public JPanel getPanel() {
		// TODO Auto-generated method stub
		return null;
	}

	@Override
	public void cameraFeaturePointsChanged(ImageSource i) {
		compute(i);

	}

}
